#include "ros/ros.h"
#include "std_msgs/String.h"
#include "nav_msgs/Path.h"
#include "sensor_msgs/PointCloud2.h"
#include  "iostream"
#include "fstream"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl_conversions/pcl_conversions.h"
#include "Eigen/Core"

using namespace Eigen;
using namespace std;

string  file_name1="/home/tt/catkin_ws/src/msg_subscriber/path.csv";
string file_name2="/home/tt/catkin_ws/src/msg_subscriber/pcl3.csv";
string file_name3="/home/tt/catkin_ws/src/msg_subscriber/pcl2.csv";

struct POSES
{
    double x;
    double y;
    double z;
};



void doPathmsgs(const nav_msgs::Path::ConstPtr& path){
    ofstream file(file_name1);
    if(!file){
        cout<<"can not find file_name1!"<<endl;
        return;
    }else{
        for(int i=0;i<path->poses.size();++i){
            file<<path->poses[i].pose.position.x<<","<<path->poses[i].pose.position.y<<"\n";
        }
    }
    file.close();
    //cout<<path->poses.size()<<endl;
}

void doMapmsgs(const sensor_msgs::PointCloud2::ConstPtr& pcl){
    // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    // pcl::fromROSMsg(*pcl,*cloud);
    // ofstream file(file_name2);
    // if(!file){
    //     cout<<"can not find file_name2!"<<endl;
    // }else{
    //     for(int i=0;i<pcl->width*pcl->height;++i){
    //         file<<cloud->points[i].x<<" "<<cloud->points[i].y<<" "<<cloud->points[i].z<<"\n";
    //     }
    // }
    // file.close();
     vector<POSES> poses;

    ifstream file(file_name2);
    if(!file){
      cout<<"can not find pcl_file at:"<<file_name2<<endl;
        return ;
     }
     while(!file.eof()){
         double x,y,z;
         file>>x>>y>>z;
        POSES pose;
         pose.x=(x+10.0)/0.1;
         pose.y=(y+10.0)/0.1;
         pose.z=0.0;
         poses.push_back(pose);
     }

     //输出测试
    //  for(int i=0;i<poses.size();++i){
    //      cout<<poses[i].x<<"\t"<<poses[i].y<<" \t"<<poses[i].z<<"\n";
    //  }

    //Matrix<double,200,200> matrix_2d=Matrix<double,200,200>::Zero();  静态高维矩阵会导致栈溢出
    Eigen::Matrix<double,Dynamic,Dynamic> matrix_nn;
    matrix_nn=Eigen::MatrixXd::Zero(200,200);
    for(int i=0;i<poses.size();++i){
        matrix_nn(poses[i].y,poses[i].x)=1; //有障碍物的地方坐标值设为1
    }
    // for(int i=0;i<200;++i){
    //         for(int j=0;j<200;++j){
    //             cout<<matrix_nn(i,j)<<"\t";
    //     }
    //     cout<<endl;
    // }
    ofstream file1(file_name3);
      if(!file1){
        cout<<"can not find file_name3!"<<endl;
    }else{
        for(int i=0;i<200;++i){
            for(int j=0;j<200;++j){
                cout<<"file input!"<<endl;
                file1<<matrix_nn(i,j)<<",";
            }
            cout<<endl;
        }
    }
    file1.close();
}



void fileinput(){
    vector<POSES> poses;

    ifstream file(file_name2);
    if(!file){
      cout<<"can not find pcl_file at:"<<file_name2<<endl;
        return ;
     }
     while(!file.eof()){
         double x,y,z;
         file>>x>>y>>z;
        POSES pose;
         pose.x=(x+10.0)/0.1;
         pose.y=(y+10.0)/0.1;
         pose.z=0.0;
         poses.push_back(pose);
     }
      Eigen::Matrix<double,Dynamic,Dynamic> matrix_nn;
    matrix_nn=Eigen::MatrixXd::Zero(200,200);
    for(int i=0;i<poses.size();++i){
        matrix_nn(poses[i].y,poses[i].x)=1; //有障碍物的地方坐标值设为1
}
cout<<"matrix_nn"<<matrix_nn<<endl;
int count=0;

ofstream file1(file_name3);
      if(!file1){
        cout<<"can not find file_name3!"<<endl;
    }else{
        for(int i=0;i<200;++i){
            for(int j=0;j<200;++j){
                //cout<<"file input!"<<endl;
                file1<<matrix_nn(i,j)<<",";
                ++count;
                cout<<count<<"\t";
            }
            file1<<"\n";
        }
    }
    file1.close();
}

int main(int argc, char *argv[])
{
    ros::init (argc,argv,"msg_subscriber");
    ros::NodeHandle nh;
    //ros::Subscriber sub1=nh.subscribe<nav_msgs::Path>("/planning/esdf",10,doPathmsgs);
    //ros::Subscriber sub2=nh.subscribe<sensor_msgs::PointCloud2>("/my_map/occupancy",10,doMapmsgs);
    fileinput();
    //ros::spin();
    return 0;
}
